#include "slepbase.h"
#include "epph.h"



void mtLogisticC(slep_matrix* A, slep_vector* y, double z, OPTS* opts,
			  slep_matrix* X, slep_vector *c_v, slep_vector* funVal, slep_vector* valueL){
	int *ind, k, i, j;
	double q, lambda;
	slep_matrix *ATb;
	slep_vector *Ax, *p_flag, *b;
	slep_vector *m1, *m2;
	assert(A->m == y->dim);
	assert(z>=0) ;
	sll_opts(opts);
	assert(hasGot(opts,SlepOPT_indices));
	ind = opts->ind;
	k = opts->ind_size - 1;
	if(hasGot(opts,SlepOPT_q)){
		assert(SLEPDoubleApproxEquals(opts->q,2.0));
		q =  opts->q;
	}else
		q = opts->q = 2.0;
	
	initNormalization(A,opts);
	ATb = slepAllocDenseMatrix(A->n, k);
	slep_zerom(ATb);
	p_flag = slepAllocDenseVector(y->dim);
	b = slepAllocDenseVector(y->dim);
	m1 = slepAllocDenseVector(k);
	m2 = slepAllocDenseVector(k);
	for(i=0; i < y->dim; ++i)
		p_flag->data.vctr[i] = (SLEPDoubleApproxEquals(y->data.vctr[i*y->inc],1.0))?1:0;
	{
		slep_vector p_flag_i = *p_flag;
		for(i=0; i<k; ++i){
			p_flag_i.data.vctr = &(p_flag->data.vctr[ind[i]]);
			p_flag_i.dim = ind[i+1] - ind[i];
			m1->data.vctr[i] = slep_dsum(&p_flag_i);
			m2->data.vctr[i] = p_flag_i.dim - m1->data.vctr[i];
		}
	}

	if( opts->init == 2){
		slep_zerom(X);
		slep_zerov(c_v);
	}else{
		if( hasGot(opts,SlepOPT_X0)){
			slep_dmcopy(opts->X0, X);
			assert(X->m == A->n && X->n == k );
		}else
			slep_zerom(X);
		if( hasGot(opts,SlepOPT_c0)){
			slep_dvcopy(opts->c0,c_v);
		}else
			for(i=0; i<k; i++)
				c_v->data.vctr[i] = log(m1->data.vctr[i]/m2->data.vctr[i]);
	}

	Ax = slepAllocDenseVector(y->dim);
	slep_zerov(Ax);

	{
		slep_matrix A_i = *A;
		slep_vector Ax_i= *Ax, x_i;
		x_i.dim = X->m;
		x_i.inc = 1;
		x_i.sparsity = SlepDense;
		for(i=0; i < k ; i++){
			A_i.m = ind[i+1] - ind[i];
			Ax_i.dim = ind[i+1] - ind[i];
			A_i.data.mtrx = &(A->data.mtrx[ind[i]]);
			Ax_i.data.vctr = &(Ax->data.vctr[ind[i]]);
			x_i.data.vctr = &(X->data.mtrx[i*(X->ld)]);
			slep_daAxpby(SlepNoTranspose,1.0, &A_i, &x_i, 0.0, &Ax_i);
		}
	}

	if( opts->lFlag==0){
		int bFlag = 0, iterStep;
		double L, alpha, alphap, beta;
		slep_matrix *Xp, *XXp, *S, *G, *V;
		slep_vector *Axp, *As;

		slep_vector *cp, *ccp, *sc, *sc_expanded;
		slep_vector *aa, *bb, *prob;
		slep_vector *gc;

		double l_sum , r_sum;
		double fun_x, fun_s;

		double lambda0;
		int steps;


		lambda0 = 0.0;
		L = 1.0/ A->m;

		Xp = slepCopyToNewMatrix(X);
		Axp = slepCopyToNewVector(Ax);
		XXp = slepAllocDenseMatrix(X->m, X->n);
		slep_zerom(XXp);
		cp = slepAllocDenseVector(c_v->dim);
		ccp = slepAllocDenseVector(c_v->dim);
		
		slep_dvcopy(c_v,cp);
		slep_zerov(ccp);
		


		sc = slepAllocDenseVector(c_v->dim);
		sc_expanded = slepAllocDenseVector(y->dim);

		aa = slepAllocDenseVector(y->dim);
		bb = slepAllocDenseVector(y->dim);
		prob = slepAllocDenseVector(y->dim);

		gc = slepAllocDenseVector(k);

		S = slepAllocDenseMatrix(X->m, X->n);
		G = slepAllocDenseMatrix(X->m, X->n);
		V = slepAllocDenseMatrix(X->m, X->n);
		As = slepAllocDenseVector(Ax->dim);
		
		alphap = 0.0;
		alpha = 1.0;
		for(iterStep = 0; iterStep < opts->maxIter ; ++iterStep){
			beta = ( alphap - 1.0)/ alpha;

			slep_dmcopy(X,S);
			slep_daXpbY(SlepNoTranspose,beta,XXp,1.0,S);

			slep_dvcopy(c_v,sc);
			slep_daxpby(beta, ccp, 1.0, sc);

			slep_dvcopy(Ax,As);
			slep_daxpby(-beta,Axp,1.0+beta,As);

			for(i=0; i<k; ++i)
				for(j=ind[i]; j<ind[i+1]; ++j)
					sc_expanded->data.vctr[j] = sc->data.vctr[i];

			fun_s = 0.0;
			for(i=0; i< y->dim; ++i){
				aa->data.vctr[i] = - y->data.vctr[i] * (As->data.vctr[i] + sc_expanded->data.vctr[i]);
				bb->data.vctr[i] = SLEPMAX(aa->data.vctr[i],0.0);
				fun_s += (log( exp(-bb->data.vctr[i])+exp(aa->data.vctr[i] - bb->data.vctr[i])) + bb->data.vctr[i]) / y->dim;
				prob->data.vctr[i] = 1.0/(1.0+exp(aa->data.vctr[i]));
				b->data.vctr[i] = - y->data.vctr[i] * (1.0 - prob->data.vctr[i] ) / y->dim ;
			}

			{
				slep_vector b_i = *b;
				for(i=0; i<k ; ++i){
					b_i.data.vctr = &(b->data.vctr[ind[i]]);
					b_i.dim = ind[i+1] - ind[i];
					gc->data.vctr[i] = slep_dsum(&b_i);
				}
			}

			// g = ATb
			{
				slep_matrix A_part = *A;
				slep_vector G_i ;
				slep_vector b_i;
				G_i.dim = A->n; 
				G_i.inc = 1;
				G_i.sparsity = SlepDense;
				b_i.inc = 1;
				b_i.sparsity = SlepDense;
				for(i=0; i<k; i++){
					A_part.data.mtrx = &(A->data.mtrx[ind[i]]);
					A_part.m = ind[i+1] - ind[i];
					G_i.data.vctr = &(G->data.mtrx[i*G->ld]);
					b_i.data.vctr = &(b->data.vctr[ind[i]]);
					b_i.dim = ind[i+1] - ind[i];
					slep_daAxpby(SlepTranspose,1.0,&A_part, &b_i, 0.0, &G_i);
				}
			}

			slep_dmcopy(X, Xp);
			slep_dvcopy(Ax, Axp);
			slep_dvcopy(c_v, cp);
			for(;;){
				// v = s - g/L;
				slep_dmcopy(S,V);
				slep_daXpbY(SlepNoTranspose,-1.0/L, G, 1.0, V);
				slep_dvcopy(sc,c_v);
				slep_daxpby(-1.0/L,gc, 1.0, c_v);
				
				ep21d(X->data.mtrx,&lambda,&steps,V->data.mtrx,X->m, X->n, z, lambda0);
				lambda0 = lambda;

				//v=x-s;
				slep_dmcopy(X,V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				{
					slep_matrix A_i = *A;
					slep_vector Ax_i= *Ax, x_i;
					x_i.dim = X->m;
					x_i.inc = 1;
					x_i.sparsity = SlepDense;
					for(i=0; i < k ; i++){
						A_i.m = ind[i+1] - ind[i];
						Ax_i.dim = ind[i+1] - ind[i];
						A_i.data.mtrx = &(A->data.mtrx[ind[i]]);
						Ax_i.data.vctr = &(Ax->data.vctr[ind[i]]);
						x_i.data.vctr = &(X->data.mtrx[i*(X->ld)]);
						slep_daAxpby(SlepNoTranspose,1.0, &A_i, &x_i, 0.0, &Ax_i);
					}
				}

				for(i=0; i<k; ++i)
					for(j=ind[i]; j<ind[i+1]; ++j)
						sc_expanded->data.vctr[j] = c_v->data.vctr[i];

				fun_x = 0.0;
				for(i=0; i< y->dim; ++i){
					aa->data.vctr[i] = - y->data.vctr[i] * (Ax->data.vctr[i] + sc_expanded->data.vctr[i]);
					bb->data.vctr[i] = SLEPMAX(aa->data.vctr[i],0.0);
					fun_x += (log( exp(-bb->data.vctr[i])+exp(aa->data.vctr[i] - bb->data.vctr[i])) + bb->data.vctr[i]) / y->dim;
				}
				r_sum = slep_dfrbnrm(V);
				r_sum = r_sum * r_sum;
				r_sum += slep_ddot(c_v,c_v) + slep_ddot(sc,sc) - 2.0 * slep_ddot(c_v,sc);
				r_sum = r_sum/ 2.0;
				l_sum = fun_x - fun_s - slep_ddot(c_v,gc) + slep_ddot(sc,gc) - slep_dtraceAB(SlepNoTranspose,V,SlepTranspose,G);
				
				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L, l_sum / r_sum);
			}
			alphap = alpha;
			alpha = ( 1.0+ sqrt(4*alpha*alpha+1.0))/2.0;
			valueL->data.vctr[iterStep] = L;
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dvcopy(c_v,ccp);
			slep_daxpby(-1.0,cp,1.0,ccp);

			funVal->data.vctr[iterStep] = fun_x;

			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(S);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeVector(Axp);
		slepFreeVector(As);


		slepFreeVector(cp);
		slepFreeVector(ccp);
		slepFreeVector(sc);
		slepFreeVector(sc_expanded);
		slepFreeVector(aa);
		slepFreeVector(bb);
		slepFreeVector(prob);
		slepFreeVector(gc);

		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}
	
	if( opts->lFlag==1 ){
		int bFlag = 0, iterStep;
		double L;
		slep_matrix *Xp, *XXp, *S, *G, *V;
		slep_vector *Axp, *As;

		slep_vector *cp, *ccp, *sc, *sc_expanded;
		slep_vector *aa, *bb, *prob;
		slep_vector *gc;

		slep_vector *cnew, *Axnew;
		slep_matrix *Xnew;

		double l_sum , r_sum;
		double fun_x, fun_s;
		double gamma = 1.0, alpha, alphap, beta, tao;

		double lambda0 ;
		int steps;

		lambda0 = 0.0;

		L = 1.0/ A->m;

		Xp = slepCopyToNewMatrix(X);
		Axp = slepCopyToNewVector(Ax);
		XXp = slepAllocDenseMatrix(X->m, X->n);
		slep_zerom(XXp);
		cp = slepAllocDenseVector(c_v->dim);
		ccp = slepAllocDenseVector(c_v->dim);
		
		slep_dvcopy(c_v,cp);
		slep_zerov(ccp);

		sc = slepAllocDenseVector(c_v->dim);
		sc_expanded = slepAllocDenseVector(y->dim);

		aa = slepAllocDenseVector(y->dim);
		bb = slepAllocDenseVector(y->dim);
		prob = slepAllocDenseVector(y->dim);

		gc = slepAllocDenseVector(k);

		S = slepAllocDenseMatrix(X->m, X->n);
		G = slepAllocDenseMatrix(X->m, X->n);
		V = slepAllocDenseMatrix(X->m, X->n);
		As = slepAllocDenseVector(Ax->dim);

		cnew = slepAllocDenseVector(c_v->dim);
		Axnew = slepAllocDenseVector(Ax->dim);
		Xnew = slepAllocDenseMatrix(X->m, X->n);


		for(iterStep = 0; iterStep < opts->maxIter ; ++iterStep){

			for(;;){
				if(iterStep!=0){
					alpha = (-gamma + sqrt(gamma*gamma + 4.0*L*gamma))/(2.0*L);
					beta = (gamma - gamma * alphap) / (alphap * gamma + alphap * L * alpha);

					slep_dmcopy(X,S);
					slep_daXpbY(SlepNoTranspose,beta,XXp,1.0,S);

					slep_dvcopy(c_v,sc);
					slep_daxpby(beta, ccp, 1.0, sc);

					slep_dvcopy(Ax,As);
					slep_daxpby(-beta,Axp,1.0+beta,As);
				}else{
					alpha = (-1.0 + sqrt(5.0))/2.0 ; 
					beta = 0.0;
					slep_dmcopy(X,S);
					slep_dvcopy(c_v,sc);
					slep_dvcopy(Ax,As);
				}

				for(i=0; i<k; ++i)
					for(j=ind[i]; j<ind[i+1]; ++j)
						sc_expanded->data.vctr[j] = sc->data.vctr[i];

				fun_s = 0.0;
				for(i=0; i< y->dim; ++i){
					aa->data.vctr[i] = - y->data.vctr[i] * (As->data.vctr[i] + sc_expanded->data.vctr[i]);
					bb->data.vctr[i] = SLEPMAX(aa->data.vctr[i],0.0);
					fun_s += (log( exp(-bb->data.vctr[i])+exp(aa->data.vctr[i] - bb->data.vctr[i])) + bb->data.vctr[i]) / y->dim;
					prob->data.vctr[i] = 1.0/(1.0+exp(aa->data.vctr[i]));
					b->data.vctr[i] = - y->data.vctr[i] * (1.0 - prob->data.vctr[i] ) / y->dim ;
				}

				{
					slep_vector b_i = *b;
					for(i=0; i<k ; ++i){
						b_i.data.vctr = &(b->data.vctr[ind[i]]);
						b_i.dim = ind[i+1] - ind[i];
						gc->data.vctr[i] = slep_dsum(&b_i);
					}
				}

				// g = ATb
				{
					slep_matrix A_part = *A;
					slep_vector G_i ;
					slep_vector b_i;
					G_i.dim = A->n; 
					G_i.inc = 1;
					G_i.sparsity = SlepDense;
					b_i.inc = 1;
					b_i.sparsity = SlepDense;
					for(i=0; i<k; i++){
						A_part.data.mtrx = &(A->data.mtrx[ind[i]]);
						A_part.m = ind[i+1] - ind[i];
						G_i.data.vctr = &(G->data.mtrx[i*G->ld]);
						b_i.data.vctr = &(b->data.vctr[ind[i]]);
						b_i.dim = ind[i+1] - ind[i];
						slep_daAxpby(SlepTranspose,1.0,&A_part, &b_i, 0.0, &G_i);
					}
				}

				// V = s - g/L;
				slep_dmcopy(S,V);
				slep_daXpbY(SlepNoTranspose,-1.0/L, G, 1.0, V);

				slep_dvcopy(sc,cnew);
				slep_daxpby(-1.0/L,gc, 1.0, cnew);

				ep21d(Xnew->data.mtrx,&lambda,&steps,V->data.mtrx,X->m, X->n, z, lambda0);
				lambda0 = lambda;

				//V=xnew-s;
				slep_dmcopy(Xnew,V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				{
					slep_matrix A_i = *A;
					slep_vector Ax_i= *Ax, x_i;
					x_i.dim = Xnew->m;
					x_i.inc = 1;
					x_i.sparsity = SlepDense;
					for(i=0; i < k ; i++){
						A_i.m = ind[i+1] - ind[i];
						Ax_i.dim = ind[i+1] - ind[i];
						A_i.data.mtrx = &(A->data.mtrx[ind[i]]);
						Ax_i.data.vctr = &(Axnew->data.vctr[ind[i]]);
						x_i.data.vctr = &(Xnew->data.mtrx[i*(Xnew->ld)]);
						slep_daAxpby(SlepNoTranspose,1.0, &A_i, &x_i, 0.0, &Ax_i);
					}
				}

				for(i=0; i<k; ++i)
					for(j=ind[i]; j<ind[i+1]; ++j)
						sc_expanded->data.vctr[j] = cnew->data.vctr[i];

				fun_x = 0.0;
				for(i=0; i< y->dim; ++i){
					aa->data.vctr[i] = - y->data.vctr[i] * (Axnew->data.vctr[i] + sc_expanded->data.vctr[i]);
					bb->data.vctr[i] = SLEPMAX(aa->data.vctr[i],0.0);
					fun_x += (log( exp(-bb->data.vctr[i])+exp(aa->data.vctr[i] - bb->data.vctr[i])) + bb->data.vctr[i]) / y->dim;
				}
				r_sum = slep_dfrbnrm(V);
				r_sum = r_sum * r_sum;
				r_sum += slep_ddot(cnew,cnew) + slep_ddot(sc,sc) - 2.0 * slep_ddot(cnew,sc);
				r_sum = r_sum/ 2.0;
				l_sum = fun_x - fun_s - slep_ddot(cnew,gc) + slep_ddot(sc,gc) - slep_dtraceAB(SlepNoTranspose,V,SlepTranspose,G);
				
				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L, l_sum / r_sum);
			}
			gamma = L * alpha * alpha;
			alphap = alpha;
			valueL->data.vctr[iterStep] = L;
			tao = L * r_sum / l_sum;
			if(tao >= 5.0)
				L=L * 0.8;
			slep_dmcopy(X,Xp);
			slep_dmcopy(Xnew,X);
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose,-1.0,Xp,1.0,XXp);
			slep_dvcopy(Ax,Axp);
			slep_dvcopy(Axnew,Ax);
			slep_dvcopy(c_v,cp);
			slep_dvcopy(cnew,c_v);
			slep_dvcopy(c_v,ccp);
			slep_daxpby(-1.0,cp,1.0,ccp);

			funVal->data.vctr[iterStep] = fun_x;

			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(S);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeVector(Axp);
		slepFreeVector(As);

		slepFreeVector(cp);
		slepFreeVector(ccp);
		slepFreeVector(sc);
		slepFreeVector(sc_expanded);
		slepFreeVector(aa);
		slepFreeVector(bb);
		slepFreeVector(prob);
		slepFreeVector(gc);

		slepFreeVector(cnew);
		slepFreeVector(Axnew);
		slepFreeMatrix(Xnew);

		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}
	slepFreeMatrix(ATb);
	slepFreeVector(Ax);
	slepFreeVector(b);
	slepFreeVector(p_flag);
	slepFreeVector(m1);
	slepFreeVector(m2);
}